// Standard includes
#include <iostream>
#include <pthread.h>
#include <semaphore.h>
#include <sys/time.h>
#include <cmath>

using std::cout;
using std::cerr;
using std::endl;
#include <vector>
using std::vector;

// Local includes
#include <KnowledgeBase.h>
#include <lidar/lidar.h>
#include <lidar/dynamixel.h>
#include <path/path.h>
#include <path/pathplanner.h>
#include <path/point.h>
#include <motion/Matrix.h>
#include <motion/Leg.h>
#include <motion/Robot.h>
#include <motion/Link.h>
#include <motion/Gait.h>
#include <motion/XYController.h>
#include <motion/CommunicationMan.h>
#include <motion/Timer.h>
#include <UrgCtrl.h>
using qrk::UrgCtrl;

// Globals
KnowledgeBase *kb;

// MAIN
int main(int argc, char* argv[]) {
        void* planner(void *);
        void* pilot(void *);
        void* sensors(void *);

	// Create KB
	kb = new KnowledgeBase();
        
	// Create threads
        pthread_t plannerThread, pilotThread, sensorsThread;

	// Planner thread
        if(pthread_create(&plannerThread, NULL, planner, NULL) != 0) {
                cerr << "Error creating planner thread.\n";
                return 1;
        }

	// Pilot thread
        if(pthread_create(&pilotThread, NULL, pilot, NULL) != 0) {
                cerr << "Error creating pilot thread.\n";
                return 1;
        }

	// Sensors thread
	if(pthread_create(&sensorsThread, NULL, sensors, NULL) != 0) {
 		cerr << "Error creating sensors thread.\n";
                return 1;
	}

	kb->setPlanPath(true);
	kb->setPlanGait(true);

	// Wait for threads
        pthread_join(plannerThread, NULL);
        pthread_join(pilotThread, NULL);
        pthread_join(sensorsThread, NULL);
	
	delete kb;

	return 0;
}

// PLANNER
void* planner(void *) {
        cout << "Planner initialized" << endl;

	// For now, plan will be to walk straight for 2m at 0.1m increments
	while(1) {
		if(kb->getPlanPath()) {
			Path* p = new Path();
			p = addToEndOfPath(p, Point(0, 0));
			p = addToEndOfPath(p, Point(0, 1));
			p = addToEndOfPath(p, Point(0, 2));
			p = addToEndOfPath(p, Point(0, 3));
			p = addToEndOfPath(p, Point(0, 4));
			p = addToEndOfPath(p, Point(0, 5));
			p = addToEndOfPath(p, Point(0, 6));
			p = addToEndOfPath(p, Point(0, 7));
			p = addToEndOfPath(p, Point(0, 8));
			p = addToEndOfPath(p, Point(0, 9));
			p = addToEndOfPath(p, Point(0, 10));
			p = addToEndOfPath(p, Point(0, 11));
			p = addToEndOfPath(p, Point(0, 12));
			p = addToEndOfPath(p, Point(0, 13));
			p = addToEndOfPath(p, Point(0, 14));
			p = addToEndOfPath(p, Point(0, 15));
			p = addToEndOfPath(p, Point(0, 16));
			p = addToEndOfPath(p, Point(0, 17));
			p = addToEndOfPath(p, Point(0, 18));
			p = addToEndOfPath(p, Point(0, 19));
			p = addToEndOfPath(p, Point(0, 20));

			kb->setPath(p);
			break; // Only need to set once
		}
	}
	
/*
	// This will be for when we actually use LIDAR data to plan path
	while(1) {
		if(kb->getPlanPath()) {
			// Filter map
			filterMap(0.1, 2);

			// Occupancy grid
			constructOccupancyGrid();
			expandOccupancyGrid();

			// Reverse occ grid
			int** cp = (int**)malloc(40 * sizeof(int *));
			for(int i=0; i<40; i++) {
				cp[i] = (int*) malloc(170 * sizeof(int));
			}

			for(int i=0; i<occHeight; i++) {
				for(int j=0; j<occWidth; j++) {
					cp[i][j] = occupancy[occHeight-1-i][j];
				}
			}

			// Path planning - points arbitrarily chosen
			Point a(occWidth/2,occHeight-1), b(occWidth/2, 0);
			PathPlanner *pp = new PathPlanner(cp, occWidth, occHeight, a, b);
			pp->astar();
			Path *p = pp->getPath();
			if(p != NULL) {
			 	kb->setPath(p);
			}
		}
	}
*/
        return (void*)0;
}

// PILOT
void* pilot(void *) {
        cout << "Pilot initialized" << endl;

	while(1) {
		if(kb->getPlanGait()) {
			Path* p = kb->getPath();
			if(p != NULL) {
				double legx[4];
				double legy[4];
				double legz[4];
				int gaitnum=1;
				int setspeed=1;

				Robot *ST = new Robot();
				Gait *GC = new Gait(ST, gaitnum,setspeed);
				XYController *XY = new XYController(GC);
				char ip[10]="127.0.0.1";
				CommunicationMan *CM = new CommunicationMan(ST,ip,5000);
				Timer *T = new Timer();

				for (int legnum = 0; legnum < LEGS; legnum++) {
					//Link 1
					XY->gait->getRobot()->legs[legnum].links[0].setLength(LINK1LENGTH);
					XY->gait->getRobot()->legs[legnum].links[0].setMass(LINK1MASS);
					Matrix CG_Link1(1, 4, 0.076, 90.0, 0.0, 90.0);
					XY->gait->getRobot()->legs[legnum].links[0].setCG(CG_Link1);
					Matrix DH_Link1(1, 4, 0.15, 90.0, 0.0, 90.0);
					XY->gait->getRobot()->legs[legnum].links[0].setDH(DH_Link1);

					//Link 2
					XY->gait->getRobot()->legs[legnum].links[1].setLength(LINK2LENGTH);
					XY->gait->getRobot()->legs[legnum].links[1].setMass(LINK2MASS);
					Matrix CG_Link2(1, 4, LINK2LENGTHCG, 0.0, 0.0, 0.0);
					XY->gait->getRobot()->legs[legnum].links[1].setCG(CG_Link2);
					Matrix DH_Link2(1, 4, LINK2LENGTH, 0.0, 0.0, 0.0);
					XY->gait->getRobot()->legs[legnum].links[1].setDH(DH_Link2);

					//Link 3
					XY->gait->getRobot()->legs[legnum].links[2].setLength(LINK3LENGTH);
					XY->gait->getRobot()->legs[legnum].links[2].setMass(LINK3MASS);
					Matrix CG_Link3(1, 4, LINK3LENGTHCG, 0.0, 0.0, 0.0);
					XY->gait->getRobot()->legs[legnum].links[2].setCG(CG_Link3);
					Matrix DH_Link3(1, 4, LINK3LENGTH, 0.0, 0.0, 0.0);
					XY->gait->getRobot()->legs[legnum].links[2].setDH(DH_Link3);

					//Front legs are 0,1
					if((legnum==0)||(legnum==1)){
						XY->gait->getRobot()->legs[legnum].setUnit(1.0);
					}
					//Back legs are 2,3
					else if((legnum==2)||(legnum==3)){
						XY->gait->getRobot()->legs[legnum].setUnit(-1.0);
					}

					//Set Initial XYZ of leg's tips
					XY->gait->getRobot()->legs[legnum].setDesiredx(0);
					double git = XY->gait->getRobot()->legs[legnum].getUnit()*-0.5*XY->gait->getLength();
					XY->gait->getRobot()->legs[legnum].setDesiredy(git);
					XY->gait->getRobot()->legs[legnum].setDesiredz(XY->gait->getLegmax());
				}
	
				double oldX = 0.0;
				double oldY = 0.0;
	
				// First point is (0, 0), so skip it
				if(p != NULL) {
					p = p->next;
				}

				int pnum = 0;
				while(p != NULL) {
					double Xpos = (double)(p->p.x) / 10;
					double Ypos = (double)(p->p.y) / 10;
					XY->setDesiredX(Xpos);
					XY->setDesiredY(Ypos);
					XY->setCurrentX(oldX);
					XY->setCurrentY(oldY);
					XY->calculateIterations();

					cout << (pnum++) << ": (" << Xpos << ", " << Ypos << ")" << endl;

					//swtemp calculates the amount of wait to achieve the proper velocity
					int swtemp=XY->gait->getSpeedWait()*1000;	
	
					//Go to position created in the XYController
					for(int i=0; i<XY->getI(); i++){
						XY->calculateVectors();

						//Go through the gait.
						for(int o=0; o<LEGS; o++){
							legx[o]=XY->gait->getRobot()->legs[o].getDesiredx();
							legy[o]=XY->gait->getRobot()->legs[o].getDesiredy();
							legz[o]=XY->gait->getRobot()->legs[o].getDesiredz();
						}
						for(int g=1; g<XY->gait->getOffset4(); g++){
							XY->gait->setGaitmotion(g); // Set the counter in the Gait

							XY->gait->calculateGait(); // Calculate where the robot should move through the gait

							//Calculate the angles necessary for the robot needs to move to!
							XY->gait->getRobot()->inverseK();

							if(g>3){
								XY->gait->logDesiredPositions();
								XY->gait->getRobot()->logIK();
							}

			
							while(swtemp>=50){
								swtemp=swtemp-50;
								CM->sendAngles();
								T->waitms(50);
							}
							if(swtemp<50){
								swtemp=swtemp+XY->gait->getSpeedWait()*1000;
							}

						}
						for(int o=0; o<LEGS; o++){
							//Set time to 0 at end of the gait, just to be sure.
							XY->gait->getRobot()->legs[o].setDesiredx(legx[o]);
							XY->gait->getRobot()->legs[o].setDesiredy(legy[o]);
							XY->gait->getRobot()->legs[o].setDesiredz(legz[o]);
						}
						CM->sendAngles();
					}

					oldX = Xpos;
					oldY = Ypos;
					p = p->next;
				}
				break;
			}
		}
	}
       
        return (void*)0;
}

// SENSORS
void* sensors(void *) {
        cout << "Sensors initialized" << endl;

	int index = 0, deviceIndex = 0, moving, presentPos, commStatus;
	int positions[] = { posHigh, posLow }, speeds[] = { speedHigh, speedLow };
	
	const char *port = "/dev/ttyACM0";

	UrgCtrl lidar;
	if (!lidar.connect(port)) {
		cerr << "Failed to connect to LIDAR!" << endl;
		return (void *)1;
	}
	int indexL = lidar.rad2index(-HALF_PI);
	int indexH = lidar.rad2index(HALF_PI);

	// Open USB2Dynamixel
	if(dxl_initialize(deviceIndex, DEFAULT_BAUD) == 0) {
		cerr << "Failed to open USB2Dynamixel!" << endl;
		return (void *)1;
	}

	// Set torque limit
	dxl_write_word(DEFAULT_ID, TORQUE_LIMIT_L, DEFAULT_TORQUE);
	while(1) {
		initializeMaps();

		// Write goal speed/position
		dxl_write_word(DEFAULT_ID, MOVING_SPEED_L, speeds[index]);
		dxl_write_word(DEFAULT_ID, GOAL_POSITION_L, positions[index]);
		do {
			// Read present position
			presentPos = dxl_read_word(DEFAULT_ID, PRESENT_POSITION_L);
			commStatus = dxl_get_result();

			// Only scan on the way down
			if(index) {
				// Convert raw position to a z-coordinate
				presentPos = posDiff - (presentPos - posHigh);
				vector<long> data;

				// Get the data
				lidar.capture(data);
				for(int j=0; j<(indexH - indexL); j++) {
					map3d[presentPos][j] = (double)(data[indexL + j]) / 1000;
				}
			}

			// Check if servo is done moving
			moving = dxl_read_byte(DEFAULT_ID, MOVING);
			commStatus = dxl_get_result();
			if(commStatus == COMM_RXSUCCESS) {
				if(!moving) {
					if(kb->getScanLidar()) {
						// Filter map
						filterMap(0.1, 2);

						// Occupancy grid
						constructOccupancyGrid();
						expandOccupancyGrid();

						// Reverse occ grid
						int** cp = (int**)malloc(40 * sizeof(int *));
						for(int i=0; i<40; i++) {
							cp[i] = (int*) malloc(170 * sizeof(int));
						}

						for(int i=0; i<occHeight; i++) {
							for(int j=0; j<occWidth; j++) {
								cp[i][j] = occupancy[occHeight-1-i][j];
							}
						}
	
						kb->setWorldMap(cp);
					}
					
					index = !index;
				}
			} else {
				break;
			}
		} while(moving);
	}
       
        return (void*)0;
}


